/*
deal_cmd.c
offer battery agv recive cmd deal with
designed by lunanting

-----------------------
2020-06-04 15:20:47
create file
-----------------------
*/

#include "deal_cmd.h"
#include "../upper.h"
#include "../stringdef.h"

#ifdef BATTERY_VERSION
/**
 * @description: void xp_upper_solve_control_debug()
 * @param const char *data : receive buf
 * @return: void
 */
int xp_upper_solve_control_debug(const char *data)
{   
    char buf[100];
    float nums = 0;
    int ret = 0;
    float speed = 0;
    float kp = 0;
    float ki = 0;
    float kd = 0;
    u8 ifusepid = 0;
    MAG_location location={0};
    UPPER_AGV_class* upper = get_return_upper_hander(); 
    if(data==NULL)
    {
        return -1;
    }
    
    if(strncmp(data,UPPER_AGV_MOVE_AHEAD,strlen(UPPER_AGV_MOVE_AHEAD)-1)==0)
    {
        println("data is :\n\r %s",data);
        ret = xp_upper_resolve_control_debug_message(data,&nums);
        if(ret != 0)
        {
            println("xp_upper_resolve_control_debug_message error");
            return 1;
        }
         println("UPPER_AGV_MOVE_AHEAD number obtained is %f\n ",nums);
        ret = xp_wheel_set_wheel_speed(nums,nums);
        if(ret != 0)
        {
              sprintf(buf,"error");
        }
        sprintf(buf,"ok");
        println("reply buf is :\n\r %s",buf);
        xp_upper_send(buf);
    }
    else if(strncmp(data,UPPER_AGV_TURN_LEFT,strlen(UPPER_AGV_TURN_LEFT)-1)==0)
    {
        println("data is :\n\r %s",data);
        ret = xp_upper_resolve_control_debug_message(data,&nums);
        if(ret != 0)
        {
            println("xp_upper_resolve_control_debug_message error");
            return 1;
        }
        println("UPPER_AGV_TURN_LEFT number obtained is %f\n ",nums);
        ret = xp_wheel_set_wheel_speed(0,nums);
        if(ret != 0)
        {
           sprintf(buf,"error");
        }
        sprintf(buf,"ok");
        println("reply buf is :\n\r %s",buf);
        xp_upper_send(buf);
    }
    else if(strncmp(data,UPPER_AGV_TURN_RIGTHT,strlen(UPPER_AGV_TURN_RIGTHT)-1)==0)
    {
        println("data is :\n\r %s",data);
        ret = xp_upper_resolve_control_debug_message(data,&nums);
        if(ret != 0)
        {
            println("xp_upper_resolve_control_debug_message error");
            return 1;
        }
        println("UPPER_AGV_TURN_RIGTHT number obtained is %f\n ",nums);
        ret = xp_wheel_set_wheel_speed(nums,0);
        if(ret != 0)
        {
           sprintf(buf,"error");
        }
        sprintf(buf,"ok");
        println("reply buf is :\n\r %s",buf);
        xp_upper_send(buf);
    }
     else if(strncmp(data,UPPER_AGV_TURN_ROUND,strlen(UPPER_AGV_TURN_ROUND)-1)==0)
    {
        println("data is :\n\r %s",data);
        ret = xp_upper_resolve_control_debug_message(data,&nums);
        if(ret != 0)
        {
            println("xp_upper_resolve_control_debug_message error");
            return 1;
        }
        println("UPPER_AGV_TURN_RIGTHT number obtained is %f\n ",nums);
        ret = xp_wheel_set_wheel_speed(nums,-nums);
        if(ret != 0)
        {
           sprintf(buf,"error");
        }
        sprintf(buf,"ok");
        println("reply buf is :\n\r %s",buf);
        xp_upper_send(buf);
    }
    else if(strncmp(data,UPPER_AGV_SMCAR_BREAK,strlen(UPPER_AGV_SMCAR_BREAK)-1)==0)
    {
          println("data is :\n\r %s",data);
        ret = xp_wheel_set_wheel_speed(0,0);
          if(ret != 0)
        {
           sprintf(buf,"error");
        }
        sprintf(buf,"ok");        
        println("reply buf is :\n\r %s",buf);
        xp_upper_send(buf);
    }
    else if(strncmp(data,UPPER_AGV_MOTOR_ENBLE,strlen(UPPER_AGV_MOTOR_ENBLE)-1)==0)
    {
          println("data is :\n\r %s",data);
        ret = xp_wheel_motor_if_enble(syn_on);
          if(ret != 0)
        {
           sprintf(buf,"error");
        }
        sprintf(buf,"ok");        
        println("reply buf is :\n\r %s",buf);
        xp_upper_send(buf);
    }
    else if(strncmp(data,UPPER_AGV_MOTOR_UNENBLE,strlen(UPPER_AGV_MOTOR_UNENBLE)-1)==0)
    {
          println("data is :\n\r %s",data);
         ret = xp_wheel_motor_if_enble(syn_off);
          if(ret != 0)
        {
           sprintf(buf,"error");
        }
        sprintf(buf,"ok");        
        println("reply buf is :\n\r %s",buf);
        xp_upper_send(buf);
    }
     else if(strncmp(data,UPPER_AGV_GET_MAG_VAULE,strlen(UPPER_AGV_GET_MAG_VAULE)-1)==0)
    {
        println("data is :\n\r %s",data);
        xp_mag_location(&location);
        aos_mutex_lock(&g_mutex_mag_navigation,AOS_WAIT_FOREVER);    
        sprintf(buf,"s:%f p:%f i:%f d:%f mag:%d middle:%d ifusepid:%d",g_robotspeed,\
        PIDMAG.Kp,PIDMAG.Ki,PIDMAG.Kd,location.mag,location.middle,g_only_proport_use);
        println("reply buf is :\n\r %s",buf);
        xp_upper_send(buf);
        aos_mutex_unlock(&g_mutex_mag_navigation);
    }
    else if(strncmp(data,UPPER_AGV_SET_MAG_MOVE,strlen(UPPER_AGV_SET_MAG_MOVE)-1)==0)
    {
        println("data is :\n\r %s",data);
        ret = xp_upper_resolve_control_debug_message(data,&nums);
        if(ret != 0)
        {
            println("xp_upper_resolve_control_debug_message error");
            return 1;
        }
        println("UPPER_AGV_SET_MAG_MOVE number obtained is %f\n ",nums);
        aos_mutex_lock(&g_mutex_mag_navigation,AOS_WAIT_FOREVER);  
        if(fabs(nums-0.00)>0.5) //启动
        {
            xp_mag_move_start_or_stop(1);
            println("mag_move_start!!!");
        }
        else
        {
            println("mag_move_stop!!!");
            xp_mag_move_start_or_stop(0);
        }
        aos_mutex_unlock(&g_mutex_mag_navigation);
        sprintf(buf,"ok");
        println("reply buf is :\n\r %s",buf);
        xp_upper_send(buf);
    }
    else if(strncmp(data,UPPER_AGV_SET_MAG_PARA,strlen(UPPER_AGV_SET_MAG_MOVE)-1)==0)
    {
        println("data is :\n\r %s",data);
        xp_upper_control_resolve_magparam(data,&speed,&kp,&ki,&kd,&ifusepid);
        aos_mutex_lock(&g_mutex_mag_navigation,AOS_WAIT_FOREVER);
        g_robotspeed = speed;
        PIDMAG.Kp = kp;
        PIDMAG.Ki = ki;
        PIDMAG.Kd = kd;
        g_only_proport_use = ifusepid;
        println("\n\r grs:%f PIDMAG.Kp:%f PIDMAG.Ki:%f PIDMAG.kd:%f ifuseonlyp %d",g_robotspeed,PIDMAG.Kp,PIDMAG.Ki,PIDMAG.Kd,g_only_proport_use);
        aos_mutex_unlock(&g_mutex_mag_navigation);  
        sprintf(buf,"ok");
        println("reply buf is :\n\r %s",buf);
        xp_upper_send(buf);
    }
	else if(strncmp(data,UPPER_AGV_GET_RFID_LABLE_VAL,strlen(UPPER_AGV_GET_RFID_LABLE_VAL)-1)==0)
    {
        println("data is :\n\r %s",data);
     	buf[0] = xp_rfid_lable_val_get();
        println("reply buf is :\n\r %s",buf);
        hal_uart_send(&(upper->uart),buf,1,10);
        xp_rfid_lable_val_reset();
    }
	else if(strncmp(data,UPPER_AGV_BY_CAR,strlen(UPPER_AGV_BY_CAR)-1)==0){
		xp_lidar_navigation_start();
		xp_upper_send("ok");
	}
	else if(strncmp(data,UPPER_AGV_IS_CAR_ROUND_END,strlen(UPPER_AGV_IS_CAR_ROUND_END)-1)==0){
		if(xp_is_lidar_navigation_end()){
			xp_upper_send("ok");
		}else{
			xp_upper_send("error");

		}
    }
	
    else
    {
        return -2;
    }
    return 1;
}


/**
 * @description: int xp_upper_resolve_control_debug_message(const char *data)
 * @param char *buf : need to resolve data
 *        float *number:get the number
 * @return: 0  success
 *         -1  fail 
 * 用于解析主板debug指令：
 * 协议暂定：
 *  agv_move_ahead  [0.05]  	以0.05速度向前运行
    agv_move_ahead  [-0.05]  	以-0.05速度向前运行也就是向后运行
    agv_turn_left [0.01]		以0.01速度原地左转
    agv_turn_right [0.01]		以0.01速度原地右转
 */
int xp_upper_resolve_control_debug_message(const char *data,float*number)
{
    char * start = 0;
    char * end = 0;
    char temf[10] = 0;
    if(data == NULL)
    {
        UPPER_LOG("data is null");
        return -1;
    }
    if((strncmp(data,UPPER_AGV_MOVE_AHEAD,strlen(UPPER_AGV_MOVE_AHEAD)-1)==0)||
        (strncmp(data,UPPER_AGV_MOVE_BACK,strlen(UPPER_AGV_MOVE_BACK)-1)==0)||
        (strncmp(data,UPPER_AGV_TURN_LEFT,strlen(UPPER_AGV_TURN_LEFT)-1)==0)||
        (strncmp(data,UPPER_AGV_TURN_RIGTHT,strlen(UPPER_AGV_TURN_RIGTHT)-1)==0)||
        (strncmp(data,UPPER_AGV_SET_MAG_MOVE,strlen(UPPER_AGV_SET_MAG_MOVE)-1)==0)||
        (strncmp(data,UPPER_AGV_TURN_ROUND,strlen(UPPER_AGV_TURN_ROUND)-1)==0)
    )
    {
        start = strstr(data,"[");
        if(start == NULL)
        {
            UPPER_LOG("agreement hava some problem");
            return -1;
        }
         end = strstr(data,"]");
         if(end == NULL)
         {
            
            UPPER_LOG("agreement hava some problem");
            return -1;
         }
         memcpy(temf,start+1,end-(start+1));
         UPPER_LOG("xp_upper_resolve_control_debug_message:\n\r%s",temf);
        *number =atof(temf);
        return 0;
    }
    else
    {
        return -1;
    }


}


/**
 * @description: xp_upper_control_resolve_magparam(char *temf, float *speed,float *pidp,float *pidi,float *pidd,u8 *ifusepid )
 * @param :temf :need to resolve str
 *         speed: agv base speed
 *         pidp: p in pid
 *         pidi: i in pid
 *         pidd: d in pid
 *         ifusepid: if only use pid 
 *   
 * @return:void 
 * for exemple:
 *   str must be  "s:[0.05]kp:[0.001]ki:[0.002]kd:[0.003]ifusepid:[0]"
 * 
 */
void xp_upper_control_resolve_magparam(char *temf, float *speed,float *pidp,float *pidi,float *pidd,u8 *ifusepid)
{
    char *start = NULL;
    char *end = NULL;
    char num[10] ={0};
    if(temf == NULL)
    {
        println("temf is NULL");
        return ;
    }
    start = strstr(temf,"s:[");
    if(start == NULL)
    {
        println("There are problems with the agreement itself");
        return;
    }
    end = strstr(temf,"]kp:");
    if(end == NULL)
    {
        println("There are problems with the agreement itself");
        return;
    }
    memcpy(num,start+3,end-(start+3));
    *speed = atof(num);
    memset(num,0,sizeof(num));
     start = strstr(temf,"kp:[");
    if(start == NULL)
    {
        println("There are problems with the agreement itself");
        return;
    }
    end = strstr(temf,"]ki:");
    if(end == NULL)
    {
        println("There are problems with the agreement itself");
        return;
    }
     memcpy(num,start+4,end-(start+4));
    *pidp = atof(num);
    memset(num,0,sizeof(num));

    start = strstr(temf,"ki:[");
    if(start == NULL)
    {
        println("There are problems with the agreement itself");
        return;
    }
    end = strstr(temf,"]kd:");
    if(end == NULL)
    {
        println("There are problems with the agreement itself");
        return;
    }
     memcpy(num,start+4,end-(start+4));
    *pidi = atof(num);
    memset(num,0,sizeof(num));

     start = strstr(temf,"kd:[");
    if(start == NULL)
    {
        println("There are problems with the agreement itself");
        return;
    }
    end = strstr(temf,"]ifusepid:");
    if(end == NULL)
    {
        println("There are problems with the agreement itself");
        return;
    }
     memcpy(num,start+4,end-(start+4));
    *pidd = atof(num);
    memset(num,0,sizeof(num));

    start = strstr(temf,"ifusepid:[");
    if(start == NULL)
    {
        println("There are problems with the agreement itself");
        return;
    }
    end = strrchr(temf,']');
    if(end == NULL)
    {
        println("There are problems with the agreement itself");
        return;
    }
     memcpy(num,start+10,end-(start+10));
    *ifusepid = atoi(num);
}


/*
deal with upper cmd 
*/
int xp_upper_user_api(const char *data,u32 len){
    return xp_upper_solve_control_debug(data);
}
#endif
//end of the file
